using System;
using Microsoft.SPOT;
using GHIElectronics.NETMF.Hardware;
using Microsoft.SPOT.Hardware;

namespace LBNHelpers
{
	/// <summary>
	/// Servo driver
	/// </summary>
	public class ServoDriver : IDisposable
	{
		/// <summary>
		/// PWM handle
		/// </summary>
		private PWM servo;

		/// <summary>
		/// Timings range
		/// </summary>
		private int[] range = new int[2];

		/// <summary>
		/// Set servo inversion
		/// </summary>
		public bool inverted = false;

		/// <summary>
		/// Used to keep track of where we are
		/// </summary>
		private double internalDegree = 0;

		/// <summary>
		/// Create the PWM pin, set it low and configure timings
		/// </summary>
		/// <param name="pin"></param>
		public ServoDriver(Cpu.Pin pin)
		{

			servo = new PWM((PWM.Pin)pin);
			servo.Set(false);

			// Typical settings
			range[0] = 1000;
			range[1] = 2000;
		}

		public void Dispose()
		{
			servo.Dispose();
		}

		/// <summary>
		/// Allow the user to set cutom timings
		/// </summary>
		/// <param name="fullLeft"></param>
		/// <param name="fullRight"></param>
		public void setRange(int fullLeft, int fullRight)
		{
			range[1] = fullLeft;
			range[0] = fullRight;
		}

		/// <summary>
		/// Disengage the servo.
		/// The servo motor will stop trying to maintain an angle
		/// </summary>
		public void disengage()
		{
			servo.Set(false);
		}

		/// <summary>
		/// Set the servo degree
		/// </summary>
		public double Degree
		{
			set
			{
				/// Range checks
				//if (value > 180)
				//    value = 180;

				//if (value < 0)
				//    value = 0;

				// Are we inverted?
				if (inverted)
					value = 180 - value;

				// Set up so that we know where we are
				internalDegree = value;

				// Set the pulse
				servo.SetPulse(20000000, (uint)map((long)value, 0, 180, range[0], range[1]) * 1000);
			}

			get
			{
				return internalDegree;
			}
		}

		/// <summary>
		/// Used internally
		/// </summary>
		/// <param name="x"></param>
		/// <param name="in_min"></param>
		/// <param name="in_max"></param>
		/// <param name="out_min"></param>
		/// <param name="out_max"></param>
		/// <returns></returns>
		private long map(long x, long in_min, long in_max, long out_min, long out_max)
		{
			return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
		}
	}
}
